//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <mantle.h>
#include <crust.h>
#include "stubio.h"
#include "kdbs.h"
#include "_sprintf.h"

#ifdef _neptune
#include <nu_api.h>
#endif

#if defined(_KDBAGENT)

#define KDBDBG 0

const int_t c_nKdbStubStackSize = 1024;

extern Breakpoint g_breakpoints[c_nNumberOfBreakpoints];
extern DLinkNode g_freeBreakpointList;
extern DLinkNode g_usedBreakpointList;
extern Boolean g_bDbgStubEntry;
extern InterruptContext *g_pCurrentCxt;

static CProcess *s_pCurrentProcess = NULL;
//
//  Because gdb will sometimes write to the stack area to execute function
//  calls, this program cannot rely on using the supervisor stack so it
//  uses it's own stack area reserved in the int array remcomStack.
//
int_t g_KdbStubStack[c_nKdbStubStackSize / sizeof(int_t)];

// g_pKdbStack, the bottom pointer of the kdb stack
int_t *g_pKdbStack = &g_KdbStubStack[c_nKdbStubStackSize / sizeof(int_t) - 1];

extern char g_outBuf[c_uBufSize + 32];
extern char g_inBuf[c_uBufSize + 32];

extern bool_t g_bDetchedInLastContinueOrNextCmd;
extern UInt32 g_uDetchedAddrInLastContinueOrNextCmd;

extern bool_t g_bLastContinue;

extern int _TextBegin, _DataBegin;

//static int_t * GetStackPtr();

static void KdbInfoQuery();

static void KdbWriteRegisters(InterruptContext *pCxt, StubContext *pKdbCxt);

static void KdbWriteOneRegister(InterruptContext *pCxt, StubContext *pKdbCxt);

static void KdbReadMem();

static void KdbWriteMem();

static void KdbTraceDebugCommandHandle(InterruptContext *pCxt,
    StubContext *pKdbCxt);

static void KdbDetachBreakPoint(InterruptContext *pCxt, StubContext *pKdbCxt);

static void KdbAddBreakPoint();

#ifdef _x86
static void KdbDetachBreakBeforTrace(UInt32 uEip);
#endif

#ifdef _arm
static UInt32 g_insbp = 0;
static UInt32 *g_addrbp = 0;
#endif

static bool_t KdbDumpBreakpointIntoList(UInt32 uAddr);

static bool_t KdbIsValidAddress(virtaddr_t virAddr);

static void KdbLoop(vector_t vector, InterruptContext *pCxt);

static bool_t g_bQuerySym = FALSE;

static void KdbInfoQuery()
{
    if (g_inBuf[1] == 'C') {
        strcat(g_outBuf, "OK");
        return;
    }

    if (!strcmp(g_inBuf, "qOffsets")) {
        strcat(g_outBuf, "Text=0;Data=0;Bss=0");
        return;
    }

    if (!strcmp(g_inBuf, "qSM::")) {
        strcat(g_outBuf, "kernel.exe");
        return;
    }

    if (!strcmp(g_inBuf, "qSy::")) {
        if (!g_bQuerySym) {

    // BUGBUG:
#ifdef _GNUC
            sprintf(g_outBuf, "qSy:kernel.exe 0x%x -s .data 0x%x;F",
                &_TextBegin, &_DataBegin);
#endif
            g_bQuerySym = TRUE;
        }
        else {
            strcpy(g_outBuf, "OK");
        }
        return;
    }
}

static void KdbWriteRegisters(InterruptContext *pCxt, StubContext *pKdbCxt)
{
    uint_t uLen, nCount;

    uLen = strlen(&g_inBuf[1]);
    if (uLen > c_uRegBytes * 2) {
        uLen = c_uRegAllBytes * 2;
    }

    nCount = uLen / 2;

    if (!HexStrToMem(&g_inBuf[1], (byte_t *)pKdbCxt, nCount)) {
        memcpy(g_outBuf, "E01", 4);
        return;
    }

    GDB_FLUSH_CACHE

    memcpy(g_outBuf, "OK", 3);

    pKdbCxt->RestoreKdbInterruptContext(pCxt, pKdbCxt);
}

static void KdbWriteOneRegister(InterruptContext *pCxt, StubContext *pKdbCxt)
{
    int_t nReg, nCount;
    byte_t arrTemp[c_uRegAllBytes];
    const char *ptr = NULL;

    ptr = ParseRegCommand(g_inBuf, &nReg, arrTemp, &nCount);

    if (ptr == NULL) {
        memcpy (g_outBuf, "E01", 4);
        return;
    }

    memcpy((ulong_t *)((uint8_t *)pKdbCxt + nReg * 4), arrTemp, nCount);

    memcpy(g_outBuf, "OK", 3);

    pKdbCxt->RestoreKdbInterruptContext(pCxt, pKdbCxt);
}

static void KdbReadMem()
{
    byte_t *pAddr;
    int_t nCount;
    const char *ptr = NULL;

    ptr = ParseMemCommand(g_inBuf, &pAddr, &nCount);

    if (ptr == NULL) {
        memcpy(g_outBuf, "E01", 4);
        return;
    }

    if (nCount > ((int_t)c_uBufSize / 2)) {
        nCount = (int_t)c_uBufSize / 2;
    }

    if (!KdbIsValidAddress((virtaddr_t)pAddr)) {
        memcpy (g_outBuf, "E03", 4);
        return;
    }

    //read the content of the memory address 'addr'
    if (MemToHexStr(pAddr, g_outBuf, nCount) == NULL) {
        memcpy (g_outBuf, "E03", 4);
        return;
    }
}

static void KdbWriteMem()
{
    byte_t *pAddr;
    int_t nCount;
    const char *ptr = NULL;

    ptr = ParseMemCommand(g_inBuf, &pAddr, &nCount);

    if (ptr == NULL) {
        memcpy(g_outBuf, "E01", 4);
        return;
    }

    if (!KdbIsValidAddress((virtaddr_t)pAddr)) {
        memcpy (g_outBuf, "E03", 4);
        return;
    }

    //Write ptr to the addr
    if (!HexStrToMem(ptr, pAddr, nCount)) {
        memcpy(g_outBuf, "E03", 4);
        return;
    }

    GDB_FLUSH_CACHE

    memcpy(g_outBuf, "OK", 2);
    g_outBuf[2] = '\0';
}

static void KdbTraceDebugCommandHandle(InterruptContext *pCxt,
    StubContext *pKdbCxt)
{
#ifdef _x86

    int_t nAddr;
    const char *ptr = NULL;

    KdbDetachBreakBeforTrace(pKdbCxt->eip);

    ptr = &g_inBuf[1];

    if (HexWordStrToInt(ptr, &nAddr) != NULL) {
        pKdbCxt->SetEip(nAddr);
    }

    if (g_inBuf[0] == 's') {
        pKdbCxt->EnableTrace();
    }
    else {
        if (g_bLastContinue) {
            pKdbCxt->EnableTrace();
        }
        else {
            pKdbCxt->DisableTrace();
        }
    }

    pKdbCxt->RestoreKdbInterruptContext(pCxt, pKdbCxt);
#endif // _x86

#ifdef _arm
    //pc point the instruction should be interrupt,
    //and next time to be execute again
    g_cpArmCurrentCxt = pKdbCxt;
    if (g_inBuf[0] == 's') {
        /*
         * we stopped, and is not caused by a 'step(i)' or 'next(i)', just save the original instruction
         */
        if (g_addrbp == 0) {
            g_addrbp = (UInt32 *)arm_target_ins((UInt32 *)pKdbCxt->pc, *(UInt32 *)pKdbCxt->pc);
            pKdbCxt->pc = (UInt32)g_addrbp;
        }
        else {
            /*
             * we stopped by a 'step(i)' or 'next(i)', we should restore the original instruction 
             * and set a new breakpoint in the next instruction.
             */
            *g_addrbp = g_insbp;
            UInt32 *addrnext = arm_target_ins(g_addrbp, (UInt32)*g_addrbp);
            pKdbCxt->pc = (UInt32)g_addrbp;
            g_addrbp = addrnext;
        }

        if (*g_addrbp == BREAK_INSTRUCTION) {
            /*
             * if next step we will step into a breakpoint, just continue to it!
             * or we will stay in the breakpoint twice: first time for the breakpoint 
             * which is set by 'step', second for the original breakpoint.
             */
            g_addrbp = 0;
        }
        else {
            g_insbp = *g_addrbp;
            *g_addrbp = BREAK_INSTRUCTION;
        }

        GDB_FLUSH_CACHE
        pKdbCxt->RestoreKdbInterruptContext(pCxt, pKdbCxt);
        return;
    }

    if (g_inBuf[0] == 'c') {
        //skip hard-coded breakpoint
        if (*(UInt32 *)pKdbCxt->pc == BREAK_INSTRUCTION) {
            pKdbCxt->pc += 4;
            GDB_FLUSH_CACHE
            pKdbCxt->RestoreKdbInterruptContext(pCxt, pKdbCxt);
        }

        if (g_addrbp == 0) return;
        *g_addrbp = g_insbp;
        pKdbCxt->pc = (UInt32)g_addrbp;

        GDB_FLUSH_CACHE
        g_addrbp = 0;
        pKdbCxt->RestoreKdbInterruptContext(pCxt, pKdbCxt);
        return;
    }
#endif
}

static void KdbDetachBreakPoint(InterruptContext *pCxt, StubContext *pKdbCxt)
{
    int_t nRet, nType, nLen;
    byte_t  *pAddr;
    Breakpoint *pTemp, *pLast;

    if (g_inBuf[1] == 'z') {
        nRet = 1;
        pLast = NULL;

#ifdef _x86
        pKdbCxt->SetEip(pKdbCxt->eip - c_uDbgInstructionLen);
#endif // _x86

#ifdef _arm
        pKdbCxt->pc = pKdbCxt->pc - BREAK_INSTRUCTION_LENGTH;
#endif

        pKdbCxt->RestoreKdbInterruptContext(pCxt, pKdbCxt);

        ForEachDLinkNode(Breakpoint *, pTemp, &g_usedBreakpointList) {
            if (!HexStrToMem(pTemp->buf, pTemp->pAddr,
                    BREAK_INSTRUCTION_LENGTH)) {
                nRet = 0;
            }

            GDB_FLUSH_CACHE

            pLast = pTemp;
        }

        if (pLast != (Breakpoint *)&g_usedBreakpointList) {
            g_freeBreakpointList.InsertFirst(pLast);
            g_usedBreakpointList.Initialize();
        }

        if (nRet) {
            memcpy(g_outBuf, "OK", 2);
        }
        else {
            memcpy(g_outBuf, "E04", 3);
        }
        return;
    }

    nRet = ParseBreakpointCommand(g_inBuf, &nType, &pAddr, &nLen);

    if (!nRet) {
        memcpy(g_outBuf, "E01", 3);
        return;
    }

    if (nType != 0) return;

    if (nLen != BREAK_INSTRUCTION_LENGTH) {
        memcpy(g_outBuf, "E02", 3);
        return;
    }

    ForEachDLinkNode(Breakpoint *, pTemp, &g_usedBreakpointList) {
        if (pTemp->pAddr == pAddr) {
            break;
        }
    }

    if (pTemp == (Breakpoint *)&g_usedBreakpointList) {
        memcpy(g_outBuf, "E03", 3);
        return;
    }

    if (!HexStrToMem(pTemp->buf, pTemp->pAddr, BREAK_INSTRUCTION_LENGTH)) {
        memcpy(g_outBuf, "E04", 3);
        return;
    }

    GDB_FLUSH_CACHE

    pTemp->Detach();
    g_freeBreakpointList.InsertFirst(pTemp);

    memcpy(g_outBuf, "Ok", 2);

#ifdef _x86
    if ((pKdbCxt->eip - c_uDbgInstructionLen) == (UInt32)pAddr) {
        pKdbCxt->SetEip(pKdbCxt->eip - c_uDbgInstructionLen);
        pKdbCxt->RestoreKdbInterruptContext(pCxt, pKdbCxt);
    }
#endif // _x86
#ifdef _arm
    if ((pKdbCxt->pc - BREAK_INSTRUCTION_LENGTH) == (UInt32)pAddr) {
        pKdbCxt->pc = (UInt32)pAddr;
        pKdbCxt->RestoreKdbInterruptContext(pCxt, pKdbCxt);
    }
#endif
}

static void KdbAddBreakPoint()
{

    int_t nRet, nType, nLen;
    byte_t *pAddr;

    nRet = ParseBreakpointCommand(g_inBuf, &nType, &pAddr, &nLen);

    if (!nRet) {
        memcpy(g_outBuf, "E01", 4);
        return;
    }

    if (nType != 0) return;

    if (nLen != BREAK_INSTRUCTION_LENGTH) {
        memcpy(g_outBuf, "E02", 4);
        return;
    }

    if (KdbDumpBreakpointIntoList((UInt32)pAddr)) {
        memcpy(g_outBuf, "OK", 3);
    }
    else {
        memcpy(g_outBuf, "E03", 4);
    }
}

#ifdef _x86
static void KdbDetachBreakBeforTrace(UInt32 uEip)
{
    Breakpoint *pTemp;

    ForEachDLinkNode(Breakpoint *, pTemp, &g_usedBreakpointList) {
        if (pTemp->pAddr == (byte_t *)uEip) {
            break;
        }
    }

    if (pTemp != (Breakpoint *)&g_usedBreakpointList) {
        if (!HexStrToMem(pTemp->buf, pTemp->pAddr,
                BREAK_INSTRUCTION_LENGTH)) {
            memcpy(g_outBuf, "E01", 4);
            return ;
        }

        GDB_FLUSH_CACHE

        pTemp->Detach();
        g_freeBreakpointList.InsertFirst(pTemp);

        g_bDetchedInLastContinueOrNextCmd = TRUE;

        g_uDetchedAddrInLastContinueOrNextCmd = uEip;
    }
}
#endif

static bool_t KdbDumpBreakpointIntoList(UInt32 uAddr)
{
    Breakpoint *pBreakpoint;

    if (uAddr == 0) {
        return FALSE;
    }

    // find the breakpoint ip
    ForEachDLinkNode(Breakpoint *, pBreakpoint, &g_usedBreakpointList) {
        if (pBreakpoint->pAddr == (byte_t *)uAddr) {
            return TRUE;
        }
    }

    if (g_freeBreakpointList.IsEmpty()) {
        return FALSE;
    }

    pBreakpoint = (Breakpoint *)g_freeBreakpointList.First();
    pBreakpoint->Detach();

    // save the content of address and write breakpoint instruction
    // into the address, "cc" is the machine instruction of the
    // instruction "int $3"

#ifdef _arm
    if ((UInt32)g_addrbp == uAddr) {
        *g_addrbp = g_insbp;
    }
#endif

    if (MemToHexStr((byte_t *)uAddr, pBreakpoint->buf,
            BREAK_INSTRUCTION_LENGTH) == NULL \
        || !HexStrToMem(BREAK_INSSTRING, (byte_t *)uAddr,
            BREAK_INSTRUCTION_LENGTH)) {
        g_freeBreakpointList.InsertFirst(pBreakpoint);
        return FALSE;
    }

    GDB_FLUSH_CACHE

    pBreakpoint->pAddr = (byte_t *)uAddr;
    g_usedBreakpointList.InsertFirst(pBreakpoint);

    return TRUE;
}

static bool_t KdbIsValidAddress(virtaddr_t virAddr)
{
    if (IsValidKernelAddress(virAddr)) return TRUE;

    if (NULL == s_pCurrentProcess) {
        return FALSE;
    }

    if (s_pCurrentProcess->m_AS.IsValidAddress((UInt32)virAddr)) {
        return TRUE;
    }

    return FALSE;
}

static void KdbLoop(vector_t vector, InterruptContext *pCxt)
{
    StubContext kdbCxt;
    StubContext *pKdbCxt = &kdbCxt;
    g_pCurrentCxt = pCxt;

    if (!g_bDbgStubEntry) g_bDbgStubEntry = TRUE;

    pKdbCxt->SetupKdbContext(pCxt, pKdbCxt);

#ifdef _x86
    if (g_bDetchedInLastContinueOrNextCmd) {

        KdbDumpBreakpointIntoList(g_uDetchedAddrInLastContinueOrNextCmd);

        g_bDetchedInLastContinueOrNextCmd = FALSE;
        g_uDetchedAddrInLastContinueOrNextCmd = 0;
    }

    if (g_bLastContinue) {
        pKdbCxt->DisableTrace();

        pKdbCxt->RestoreKdbInterruptContext(pCxt, pKdbCxt);

        g_bLastContinue = FALSE;
        return;
    }
#endif // _x86

    GetTargetCurrentState(vector, pKdbCxt);

    AtomicPutDebugPacket(g_outBuf);

    for(;;) {
        g_outBuf[0] = '\0';

        AtomicGetDebugPacket(g_inBuf);

        if (KDBDBG) {
            DebugPrint("\nKDBSTUB_receive = ", g_inBuf);
        }

        switch (g_inBuf[0]) {
            case '?':
                GetTargetCurrentState(vector, pKdbCxt);
                break;

            case 'g':
                MemToHexStr((byte_t *)pKdbCxt, g_outBuf, c_uRegAllBytes);
                break;

            case 'q' :
                KdbInfoQuery();
                break;

            case 'H' :
                memcpy(g_outBuf, "OK", 3);
                break;

            case 'G':
                KdbWriteRegisters(pCxt, pKdbCxt);
                break;

            case 'P':
                KdbWriteOneRegister(pCxt, pKdbCxt);
                break;

            case 'm':
                KdbReadMem();
                break;

            case 'M':
                KdbWriteMem();
                break;

            case 'Z':
                KdbAddBreakPoint();
                break;

            case 'z':
                KdbDetachBreakPoint(pCxt, pKdbCxt);
                break;

            case 'c':
                g_bLastContinue = TRUE;

            case 's':
#ifdef _x86
                if (!IsValidKernelAddress((virtaddr_t)pKdbCxt->eip)) {
                    return;
                }
#endif // _x86
#ifdef _arm
                if (!IsValidKernelAddress((virtaddr_t)pKdbCxt->pc)) {
                    return;
                }
#endif

                KdbTraceDebugCommandHandle(pCxt, pKdbCxt);
                return;

            case 'k' :
            case 'D' :
                g_bDbgStubEntry = FALSE;
                BspReboot();
                break;
            default:
                break;
        }//End switch

        if (KDBDBG)
        {
            DebugPrint("\nKDBSTUB_send = ", g_outBuf);
        }

        AtomicPutDebugPacket(g_outBuf);

    }//End for
}

#if 0
static int_t * GetStackPtr()
{
    return g_pKdbStack;
}
#endif

#endif // #if defined(_KDBAGENT)

void KdbHandler(vector_t vector, InterruptContext *pCxt)
{

#if defined(_KDBAGENT)
    s_pCurrentProcess = GetCurrentProcess();

#if 0
    SwitchToKdbStack((UInt32)g_pKdbStack);
#endif

    KdbLoop(vector, pCxt);

#else

    kputs("kernel panic (KDB disabled).\n");
#ifdef _neptune
    nu_EnableVibrator(NULL, 1);
    DzDelay(10 * 1000000);
    nu_EnableVibrator(NULL, 0);
#endif
    BspHalt();
#endif
}
